/*
 * Shooter.cpp
 *
 *  Created on: Dec 27, 2012
 *      Author: trwulff
 */
#ifndef SHOOTER_CPP
#define SHOOTER_CPP 
//#define SHOOTER_MOTER_MODE_SPEED
#define SHOOTER_MOTER_MODE_VOLTAGE
#include "Shooter.hpp"


/*
 * Default constructor
 */


Shooter::Shooter():

	ShooterSolenoid1(SHOOTERID1),
	ShooterSolenoid2(SHOOTERID2),
	DropSolenoid(DROP_CYLINDERID),
	DropSolenoid2(DROP_CYLINDERID2),
#ifndef NO_SHOOTER_MOTORS
#ifdef SHOOTER_MOTER_MODE_SPEED
	ShooterMoter1(SHOOTERMOTORID1,CANJaguar::kSpeed),
	ShooterMoter2(SHOOTERMOTORID2,CANJaguar::kSpeed),
#endif
#ifdef SHOOTER_MOTER_MODE_VOLTAGE
	ShooterMoter1(SHOOTERMOTORID1,CANJaguar::kPercentVbus),
	ShooterMoter2(SHOOTERMOTORID2,CANJaguar::kPercentVbus),
#endif	
#endif
	
	ShooterTimer(),
	LaunchTimer()	
{


	printf("%s %s %s\n",__FILE__,__DATE__,__TIME__);
#ifndef NO_SHOOTER_MOTORS
	ShooterMoter1.EnableControl();
	ShooterMoter2.EnableControl();
#endif
	ShooterDelay=SHOOTERDELAY;
	LaunchDelay=SHOOTERLAUNCEDELAY;
	DropSolenoid.Set(false);
	ShotCounter=0;
	ShotCountFlg=true;
	launch=false;
	ShooterSolenoid1.Set(false);
	ShooterSolenoid2.Set(true);


	/*SmartDashboard::init();
	SmartDashboard::PutBoolean("Calibrate Shooter",false);
	SmartDashboard::PutNumber("Motor 1",0);
	SmartDashboard::PutNumber("Motor 2",0);
	SmartDashboard::PutBoolean("Launch",false);
	SmartDashboard::PutNumber("Shoot Delay",SHOOTERDELAY);
	SmartDashboard::PutNumber("Launch Delay",SHOOTERLAUNCEDELAY);
	SmartDashboard::PutBoolean("CFlg",false);
	SmartDashboard::PutBoolean("LFlg",false);
	SmartDashboard::PutBoolean("Drop",false);
*/
	



#ifdef SHOOTER_MOTER_MODE_SPEED
	ShooterMoter1.SetPID(P,I,D);
	ShooterMoter1.SetPID(P,I,D);
	ShooterMoter1.ConfigEncoderCodesPerRev(ENCODER_LPR);
	ShooterMoter2.ConfigEncoderCodesPerRev(ENCODER_LPR);
	ShooterMoter1.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
	ShooterMoter2.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
#endif

#ifndef NO_SHOOTER_MOTORS
	ShooterMoter1.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
	ShooterMoter2.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
	ShooterMoter1.SetExpiration(30);
	ShooterMoter2.SetExpiration(30);
	ShooterMoter1.SetSafetyEnabled(false);
	ShooterMoter2.SetSafetyEnabled(false);
#endif
	shoot = false ;
	motor1Speed = 0 ;
	motor2Speed = 0 ;
	
	ShooterTimer.Reset();
	ShooterTimer.Stop();
	LaunchTimer.Reset();
	LaunchTimer.Start();
	
	} 
void Shooter::Launch(){
	if(!launch)
	{
//		LaunchTimer.Reset();
		LaunchTimer.Start();
		launch=true;

	}
	if ((LaunchTimer.HasPeriodPassed(LaunchDelay)))
	{
		ShooterTimer.Reset();
		ShooterTimer.Start();
    shoot = true;	
 	}
	else
	{
		launch=false;
	}

}
void Shooter::SetSpeed(double SpeedIn){

#ifndef NO_SHOOTER_MOTORS
motor1Speed = SpeedIn;
	motor2Speed = SpeedIn;
#endif
	
	

}
void Shooter::SetSpeed(double Speed1,double Speed2){
#ifndef NO_SHOOTER_MOTORS

	ShooterMoter1.EnableControl();
	ShooterMoter2.EnableControl();
	motor1Speed = Speed1;
	motor2Speed = Speed2;
//	SmartDashboard::PutNumber("Set Speed 1",motor1Speed);
//	SmartDashboard::PutNumber("SetSpeed 2",motor2Speed);
#endif

}


	
/*
 * Default destructor
 */
 Shooter::~Shooter()
{

}

void Shooter::Update()
// The update function is expected for every class and must be called each cycle of the command loop.
// This function sould contain any motor calls. This will keep the motors functioning.
{
	
	if(shoot)
	{
		if(ShotCountFlg){ShotCounter++;ShotCountFlg=false;}
		ShooterSolenoid1.Set(true);
		ShooterSolenoid2.Set(false);
	if(ShooterTimer.HasPeriodPassed(ShooterDelay))
	{
		ShooterTimer.Stop();
		ShooterSolenoid1.Set(false);
		ShooterSolenoid2.Set(true);
		shoot = false;
	}
}
	else
	{
		ShotCountFlg=true;
	}
#ifndef NO_SHOOTER_MOTORS

	ShooterMoter1.Set(motor1Speed);
	ShooterMoter2.Set(motor2Speed);
	
//	SmartDashboard::PutNumber("Speed 1",motor1Speed);
//	SmartDashboard::PutNumber("Speed 2",motor2Speed);
#endif
}
void Shooter::Drop(bool State)
{
	DropSolenoid.Set(State);
	DropSolenoid2.Set(!State);

}

bool Shooter::HaveShot(UINT8 count)
{
	return ShotCounter >=count;
}
UINT8 Shooter::ShotCount()
{
	return ShotCounter;
}
void Shooter::ResetShotCount()
{
	ShotCounter=0;
}
void Shooter::SetShotCount(UINT8 count)
{
	ShotCounter=count;
}

bool Shooter::Calibrate()
{
//	if(	SmartDashboard::GetBoolean("Calibrate Shooter")){
		
#ifndef	NO_SHOOTER_MOTORS
//		//Shooter::SetSpeed(SmartDashboard::GetNumber("Motor 1"),SmartDashboard::GetNumber("Motor 2"));
#endif
		//SmartDashboard::PutBoolean("Launch Act",shoot);
		ShooterDelay=SmartDashboard::GetNumber("Shoot Delay");
		LaunchDelay=SmartDashboard::GetNumber("Launch Delay");
		/*if(	SmartDashboard::GetBoolean("Drop"))
		{
			Drop(true);
		}
		else
		{
			Drop(false);
		}
*/
		Shooter::Update();
//		SmartDashboard::PutBoolean("CFlg",true);

/*	if(	SmartDashboard::GetBoolean("Launch"))
	{
		Shooter::Launch();
		SmartDashboard::PutBoolean("LFlg",true);
	}
	else
	{
		SmartDashboard::PutBoolean("LFlg",false);
	}
	return true;
	}
	SmartDashboard::PutBoolean("CFlg",false);*/
#ifndef NO_SHOOTER_MOTORS
	Shooter::SetSpeed(0,0);
#endif
	return false;

}
#endif
